//
// Created by 刘洋 on 2024/2/8.
//
#include "Lowpassfilter.h"



static float Tf;
static unsigned long timestamp_prev;  //!< 最后执行时间戳
static float y_prev; //!< 上一个循环中的过滤后的值
void lowPassFilterInit(float tf)
{
    Tf = tf;
    timestamp_prev = micros();
}
float lowPassFilter(float x)
{
    unsigned long timestamp = micros();
    float dt = (timestamp - timestamp_prev)*1e-6f;

    if (dt < 0.0f ) dt = 1e-3f;
    else if(dt > 0.3f) {
        y_prev = x;
        timestamp_prev = timestamp;
        return x;
    }

    float alpha = Tf/(Tf + dt);//tf越大越相信滤波值,越小越相信实际值
    float y = alpha*y_prev + (1.0f - alpha)*x;
    y_prev = y;
    timestamp_prev = timestamp;
    return y;
}
